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1.  Introduction 


One  of  the  earliest  and  most  elusive  goals  of  robotics  has  been  the  ability  to 
program  manipulators  at  the  level  of  task  operations  rather  than  that  of  individual 
motions.  What  at  first  appeared  to  be  a  relatively  simple  problem  was  soon 
discovered  to  have  unsuspected  depths.  As  a  result,  several  proposals  to  develop 
such  a  task-level  robot  programming  system  have  not  culminated  in  a  working 
system.  Nevertheless  attempts  to  implement  the  proposals  havo  led  to  crisper 
problem  statements,  better  algorithms  and,  above  all,  they  have  provided  a  useful 
framework  for  research.  Over  the  past  six  or  seven  years,  and  partly  in  response 
to  difiUculties  encountered  implementing  these  proposals,  significant  advances  have 
been  made  in  the  theory  and  practice  of  task-level  programming.  As  a  result  of  new 
developments,  the  architecture  of  the  proposed  systems  is  no  longer  an  adequate 
framework  for  research. 

In  this  paper  we  propose  an  architecture  for  a  new  task-level  system,  which  we 
call  TWAIN.  In  contrast  to  earlier  proposals,  much  of  the  theoretical  underpinning 
of  TWAIN  exists,  and  many  of  the  components  have  been  implemented  and  tested 
as  stand-alone  systems  in  our  laboratory.  Although  these  components  require 
additional  work,  we  believe  they  provide  a  solid  basis  for  a  new  effort  at  integration. 
We  have  two  goals  for  this  work.  The  first  is  to  integrate  some  individual  pieces 
of  research  in  task  planning,  whose  relationship  has  not  previously  been  explored. 
The  second  is  to  provide  a  framework  for  further  research  in  task-planning. 

In  this  section,  we  provide  an  overview  of  task-level  programming.  Section  2 
outlines  the  architecture  of  TWAIN.  Section  3  through  5  describe,  with  the  TWAIN 
framework,  approaches  to  the  key  task-planning  modules  of  TWAIN. 

1.1.  Levels  of  Robot  Programming 

Most  robots  are  programmed  manually  by  moving  them  through  a  sequence 
of  desired  positions,  recording  the  internal  joint  coordinates  corresponding  to  each 
position,  and  then  recording  the  operations,  such  as  closing  a  gripper  or  activating 
a  welding  gun,  at  those  positions.  The  resulting  program  is  a  sequence  of  vectors  of 
joint  coordinates  and  activation  signals  for  external  equipment.  These  programs  are 
executed  by  moving  the  robot  through  the  specified  sequence  of  joint  coordinates 
and  issuing  the  indicated  signals.  This  method  of  robot  programming  is  known  as 
teaching  by  showing,  or  guiding. 

Guiding  is  simple  to  use  and  to  implement  but  subject  to  important  limitations, 
particularly  in  the  use  of  sensors.  Because  guiding  specifies  a  single  execution 
sequence  for  the  robot,  there  can  be  no  loops,  conditionals,  or  computations.  In  some 
applications,  such  as  spot  welding,  painting,  and  the  handling  of  simple  materials, 
this  is  enough.  To  do  other  applications,  including  mechanical  assembly  and 
inspection,  the  robot  must  respond  to  sensory  input,  data  retrieval,  or  computation. 
This  type  of  programming  requires  the  capabilities  of  a  general-purpose  computer 
programming  language. 
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Robot-level  languages  are  traditional  computer  programming  languages  that 
have  been  extended  with  commands  to  access  sensors  and  to  specify  robot  motions. 
Data  from  external  sensors,  such  as  vision  and  force,  may  be  used  to  modify  the 
robot’s  motions,  enabling  the  robot  to  operate  with  greater  uncertainty  in  the 
position  of  external  objects,  thereby  increasing  their  range  of  application.  The  key 
disadvantage  of  robot-level  programming  languages,  relative  to  guiding,  is  that 
they  require  the  robot  programmer  to  be  expert  in  computer  programming  and  in 
the  design  of  sensor- based  motion  strategies. 

Task-level  programming  simpHGes  the  robot  programming  process  by  requiring 
that  the  user  specify  goals  for  the  physical  relationships  among  objects,  rather  than 
the  motions  of  the  robot  needed  to  achieve  those  goals.  The  task-level  specification 
is  robot  independent,  in  that  no  position  or  path  that  depends  on  the  robot 
geometry  or  kinematics  is  specified  by  the  user.  Because  task-level  programming 
systems  require  complete  geometric  models  of  the  environment  and  of  the  robot  as 
input,  they  are  also  referred  to  as  world-modeling  systems. 

Task-level  programming  is  one  of  the  earliest  and  most  elusive  goals  of 
Robotics  Science.  What  at  first  appeared  to  be  a  relatively  simple  problem  was  soon 
discovered  to  have  unsuspected  depths.  As  a  result,  several  attempts  to  develop 
task-level  robot  programming  systems  have  not  culminated  in  working  systems. 
Nevertheless,  attempts  to  implement  the  proposed  systems  have  led  to  crisper 
problem  statements,  better  algorithms,  and  useful  frameworks  for  research.  It  is 
partly  a  result  of  these  signifeant  advances  in  the  theory  and  practice  of  task-level 
robot  programming  that  we  propose  to  develop  a  new  task-level  system. 

1.2.  Basic  problems  in  task  planning 

In  task-level  programming,  the  task  planner  converts  the  user’s  specification 
of  a  task  into  a  robot- level  program  to  carry  out  the  task^  The  main  role  of  the 
task  planner  is  to  plan  the  robot-specific  motion  and  sensing  commands  necessary 
to  achieve  the  task. 

C  nsider  a  simple  block-stacking  example  (figure  1).  The  task  can  be  specified 
as  follows: 

PLACE  A  SUCH  THAT  (A. 4  AGAINST  TABLE)  AND 

(A.l  AGAINST  F.l)  AND  (A. 2  AGAINST  F.2) 

PLACE  B  SUCH  THAT  (B.4  AGAINST  A. 3)  AND 

(A.l  COPLANAR  B.l)  AND  (A. 2  COPLANAR  B.2) 

In  the  absence  of  positioning  errors  in  the  manipulator  or  in  our  knowledge  of  the 
position  of  parts,  this  task  could  be  accomplished  with  the  following  program: 


'Wo  iL'i.suinc  Ih.it  thn  input  description  of  the  task  completely  specifies  the  .sequence  of  a.s.sembly. 
Alternatively,  another  progr.-vtn  called  an  atnembly  planner  produces  such  a  description  from  the 
user’s  description. 
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Figure  1.  A  block-stacking  example. 


1.  OPEN-FINGERS  TO  <width  of  A  +  epsilon> 

2.  MOVE  TO  <location  of  grasp  on  A>  VIA  <path> 

3.  CLOSE-FINGERS  TO  <width  of  A  -  epBilon> 

4.  MOVE  TO  <location  at  F>  VIA  <path> 

E.  OPEN-FINGERS  TO  <width  of  B  epsllon> 

6.  MOVE  TO  <location  of  grasp  on  B>  VIA  <path> 

7.  CLOSE-FINGERS  TO  <width  of  B  -  epBilon> 

8.  MOVE  TO  <location  on  A>  VIA  <path> 

9.  OPEN-FINGERS  TO  <width  of  B  ♦  epsilon> 

To  generate  this  program,  the  task  planner  chooses  positions  for  objects  initially  in 
the  workspace,  determines  how  new  objects  will  be  fed  into  the  workspace,  picks 
grasp  points  on  parts,  and  finds  paths  that  avoid  collisions.  These  problems  are 
not  the  only  ones  that  face  a  task  planner.  In  practice,  this  simple  program  fails 
to  achieve  the  desired  goals  because  of  the  presence  of  errors  in  the  robot  control 
system  and  in  the  system’s  knowledge  of  the  location  of  the  parts.  The  use  of 
sensing  can,  in  many  cases,  overcome  these  problems,  but  this  requires  that  the 
task  planner  decide  what  kind  of  sensing  is  useful  and  how  to  combine  it  with 
the  appropriate  motions.  In  fact,  dealing  with  uncertainty  permeates  all  c!*  task 
planning. 

In  summary,  to  convert  a  task-level  specification  to  a  robot-level  specification, 
a  realistic  task  planner  must  solve  the  following  problems: 

-•  Parts  feeding  —  The  planner  must  choose  how  to  introduce  each  of  the  parts 
required  for  the  assembly  into  the  workspace  in  ways  that  maximize  speed  and 
reliability  in  acquiring  the  parts.  In  the  example  in  figure  1,  the  type  of  feeder 
for  A  and  B  must  be  determined. 

Layout  —  The  planner  must  choose  where  in  the  workspace  each  operation 


is  to  take  place  in  ways  that  minimize  sensing  and  positioning  error  as  well 
as  reduce  the  time  for  the  complete  assembly.  In  the  example  in  figure  1,  the 
location  of  F  and  of  the  feeders  for  A  and  B  must  be  chosen. 

•  Fixturing  —  The  planner  must  choose  jigs  and  fixtures  to  hold  the  parts  to 
the  required  accuracy  under  the  forces  generated  by  assembly  motions.  In  the 
example  in  figure  1,  F  serves  the  function  of  a  fixture;  no  further  fixturing  is 
needed. 

•  Fine  motion  —  The  planner  must  choose  a  strategy  of  sensing  and  motion 
that  guarantees  that  parts-mating  operations  will  be  reliable  despite  errors  in 
control  and  sensing.  In  the  example  in  figure  1,  strategies  must  be  chosen  that 
guarantee  that  A  reaches  the  corner  of  F  and  that  B  is  aligned  with  A. 

•  Grasping  —  The  planner  must  choose  how  to  grasp  each  part  so  the  grasp 
is  stable,  avoiding  collisions  while  grasping  or  while  placing  the  part  at  its 
destination.  In  the  example,  the  planner  must  ensure  that  the  grasp  points  on 
A  and  B  are  stable,  enabling  the  assembly  operations  to  take  place,  and  that 
the  grasping  motions  do  not  introduce  too  much  error  in  position. 

•  Gross  motion  —  The  planner  must  choose  efficient  collision-free  paths  for  the 
manipulator  and  the  parts  it  carries.  In  the  example  in  figure  1,  the  path 
taking  A  from  the  feeder  to  near  F  and  taking  B  from  the  feeder  to  above  A 
is  chosen  to  avoid  collisions  of  the  manipulator  with  F  and  the  feeder  (even  in 
the  presence  of  position  control  error). 

One  possible  program  for  the  block-stacking  task  (taking  into  account  the 

likelihood  of  errors)  has  the  following  structure; 


1.  OPEN-FINGERS  TO  <width> 

2.  MOVE  TO  <location  of  grasp  on  A>  VIA  <path> 

3.  CLOSE-FINGERS  TO  <wldth> 

4.  MOVE  TO  <approach  location  near  F>  VIA  <path> 

5.  COMPLIANT-MOVE  ALONG  <direction>  UNTIL  <contact  with  F.l  t  F.2> 

6.  JMPLIANT-MOVE  ALONG  <direction>  UNTIL  <contact  with  TABLE> 

7.  OPEN-FINGERS  TO  <width> 

8.  VISION-LOCATE  B  NEAR  <expected  location  of  B> 

9.  OPEN-FINGERS  TO  <width> 

10.  MOVE  TO  <location  of  grasp  on  B>  VTA  <path> 

11.  CLOSE-FINGERS  TO  <width> 

12.  VISION-LOCATE  B  NEAR  <expected  location  of  B  in  fingers> 

13.  MOVE  TO  <approach  location  near  B>  VIA  <path> 

14.  COMPLIANT-MOVE  ALONG  <direction>  UNTIL  <contact  with  A.3> 


Note  that  a  simple  task-level  description  leads  to  a  complex  robot- level  program 
because  of  the  presence  of  positioning  errors  and  uncertainty.  In  fact,  this  program 
is  inadequate  for  the  task  because  it  does  not  take  into  account  the  likelihood 
that  some  operation  will  fail  due  to  unexpected  events.  No  program  ever  handles 
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all  possible  eventualities,  but  the  addition  of  sensing  significantly  increases  the 
reliability  of  the  operation,  usually  at  the  expense  of  speed.  This  example  points 
out  the  difficulty  of  robot-level  programming  and  the  potential  value  of  ta.‘;k-level 
robot  programming. 

Many  of  the  decisions  that  had  to  be  made  in  synthesizing  the  example  program 
arc  not  obvious  from  a  first  glance  at  the  program.  These  complex  decisions  manifest 
themselves  only  in  the  numeric  values  of  positions,  that  is,  in  the  positions  of  feeders 
and  grasp  points,  the  width  of  finger  openings,  directions  of  compliant  motions, 
and  the  paths  for  positioning  motions.  The  basic  program  structure  itself  —  the 
need  for  the  compliant  motions  in  steps  5  and  6  and  the  sensing  operation  at  steps 
8  and  12  —  is  based  on  numerical  estimates  of  errors  in  sensing  and  control  (partly 
based  on  the  result  of  previous  decisions).  These  decisions  are  tightly  inter-related 
and  propagate  across  what,  on  the  surface,  appear  to  be  independent  operations. 
The  choice  of  grasp  points  affects  the  assembly  operations,  the  need  for  sensing, 
and  the  choice  of  paths. 

'^he  design  of  a  planner  capable  of  transforming  a  task-level  specification  into 
a  detailed  robot  program  is  complex.  Our  approach  to  the  design  is  based  on  a  set 
of  key  ideas  and  methods: 

•  ".’ho  use  of  a  small  number  of  powerful  planning  modules  to  identify  the  range 
of  possible  values  of  the  parameters  needed  for  grasping,  gross  motion,  and 
fine  motion. 

•  The  use  constraint  propagation  to  choose  feasible  values  for  parameters  that 
affect  more  than  one  operation. 

•  The  use  of  skeleton  programs  to  indicate  stereotyped  sequences  of  operations 
required  to  execute  common  tasks. 

•  The  use  of  configuration  space  to  reason  about  legal  robot  motions  for  grasping, 
fine  motion,  and  gross  motion. 

1.3.  Hierarchical  decomposition  and  robot  programming 

Decomposition  into  independent  sub-problems,  especially  hierarchical  decom¬ 
position,  is  one  of  the  most  common  and  powerful  of  conceptual  tools  for  problem¬ 
solving.  It  is  no  surprise  that  it  forms  the  foundation  of  almost  all  programming 
methodologies.  Structured  programming,  for  example,  is  essentially  an  endorsement 
of  hierarchical  dccompos-  on  in  programs.  More  recent  developments  in  program¬ 
ming  languages,  such  as  data  encapsulation,  apply  this  approach  for  data  structures. 
Of  course,  even  ihe  simplest  programming  tasks  are  not  completely  decomposable; 
side- effect.'^,  such  as  modification  of  databases,  propagate  dependencies  across  opera¬ 
tions.  The  driving  force  behind  most  programming  methodologies  is  to  minimize 
these  dependencies. 

It  is  natural  to  attempt  to  apply  methodologies  based  on  hierarchical 
decomposition  to  robot  programming  and  control.  However,  the  fundamental 
character  of  robot  operations  limits  the  scope  of  such  decomposition.  The  key 
difficulties  are  error  and  geometry,  both  of  which  arc  non-local.  The  choice  of 


grasp  point  on  a  part,  for  example,  determines  what  motions  of  the  hand  are 
required  to  position  the  part.  The  choice  of  grasp  point,  in  turn,  is  determined 
by  subsequent  assembly  operations;  for  example,  a  finger  cannot  be  on  a  surface 
that  is  to  be  against  another  surface.  Nor  can  the  axis  between  the  fingers  be 
perpendicular  to  large  applied  forces  because  only  friction  is  holding  the  object  along 
that  direction.  Similarly,  the  existence  of  a  path  to  a  destination  might  depend  on 
how  the  part  is  held  in  the  hand.  Finally,  each  operation  makes  certain  assumptions 
about  accuracy  in  locations  and  shape  which  affect  subsequent  operations.  These 
dependencies  conspire  to  make  robot  operations  appear  monolithic;  one  often 
concludes  that  everything  must  be  decided  before  anything  at  all  is  decided. 

The  use  of  sensing  and  compliance  increases  the  class  of  situations  in  which 
particular  operations  will  succeed  and,  thereby,  reduces  inter-dependence  among 
operations.  In  the  extreme  case,  each  operation  can  sense  the  complete  current 
environment  and  decide  independently  which  method  is  adequate  to  perform  its 
task.  This  is  extremely  wasteful  even  assuming  that  the  sensing  is  free;  it  will 
require  frequent  re-grasping,  for  example.  In  general,  the  low-level  decisions  made 
in  carrying  out  each  task- level  step  (layout,  grasping,  paths,  and  sensing)  influence 
the  decisions  that  need  to  be  made  for  steps  before  and  after  this  one  [Brooks 
82,  Taylor  76].  Because  of  this,  TWAIN  is  based  on  a  two-level  approach  to  task 
planning  based  on  propagating  constraints  on  physical  quantities. 

The  TW.\!N  approach  to  robot  program  synthesis  starts  by  expanding  the  task- 
level  description  into  a  skeleton  program.  This  skeleton  program  makes  reference  to 
quantities  provided  in  the  input,  quantities  that  must  be  chosen  by  the  system,  and 
error  quantities  on  which  only  bounds  are  available.  Symbolic  algebraic  constraints 
are  used  to  make  explicit  the  interactions  among  the  quantities  across  program 
steps.  The  goal  of  the  system  is  to  make  few  arbitrary  decisions,  but  instead  to 
exploit  the  mutual  constraints  among  steps  to  force  decisions.  Each  planning  module 
restricts  the  values  of  variables  in  the  skeleton  plan  in  addition  to  determining  the 
actual  operations  needed  to  achieve  a  step  in  the  program.  The  planner  propagates 
the  restrictions  on  variables  across  operations:  forwards  from  constraints  on  input 
qua)  ities  to  constraints  on  output  quantities  and  backwards  from  constraints  on 
outj  ut  quantities  to  constraints  on  input  quantities  [Brooks  82). 

1.4.  Modeling  Requirements 

Task- level  planners  require  a  complete  world  model  and  a  complete  task 
specification. 

1.4.1.  The  World  Model 

The  legal  motions  of  an  object  are  constrained  by  the  presence  of  other  objects 
in  the  environment,  and  the  form  of  the  constraints  depends  in  detail  on  the  shapes 
of  the  objects.  Therefore,  a  task  planner  needs  complete  geometric  descriptions 
of  objects.  There  are  additional  constraints  on  motion  imposed  by  the  kinematic 
structure  of  the  robot  itself.  If  the  robot  is  turning  a  crank  or  opening  a  valve, 
then  the  kinematics  of  the  crank  and  the  valve  impose  additional  restrictions 


on  the  robot’s  motion.  The  kinematic  models  provide  the  task  planner  w  th  the 
information  required  to  plan  manipulator  motions  that  are  consistent  with  external 
constraints.  Note  that  as  a  result  of  the  robot’s  operation,  new  linkages  may  be 
created  and  old  linkages  destroyed;  the  task-planner  must  be  appraised  of  the 
changes. 

In  planning  robot  operations,  many  of  the  physical  characteristics  of  objects 
play  important  roles.  The  mass  and  inertia  of  parts,  for  example,  determine  how 
fast  they  can  be  moved  or  how  much  force  can  be  applied  to  them  before  they 
fall  over.  Similarly,  the  coefTicient  of  friction  between  a  peg  and  a  hole  affects  the 
jamrr  ing  conditions  during  insertion.  Likewise  the  physical  constants  of  the  robot 
links  are  used  in  the  dynamics  computation  and  in  the  control  of  the  robot. 

".^he  feasible  operations  of  a  robot  are  not  sufficiently  characterized  by  its 
geometrical,  kinematical,  and  physical  descriptions.  One  important  additional 
aspect  of  a  robot  system  is  its  sensing  capabilities  of  touch,  force,  and  vision.  For 
task-planning  purposes,  vision  enables  obtaining  the  configuration  of  an  object  to 
some  specified  accuracy  at  execution  time;  force  sensing  allows  the  use  of  compliant 
motions;  touch  information  serves  in  both  capacities.  In  addition  to  sensing,  there 
are  many  individual  characteristics  of  manipulators  that  must  be  described;  velocity 
and  acceleration  bounds,  positioning  accuracy  of  each  of  the  joints,  and  workspace 
bounds  are  examples. 

1.4.2.  The  Task  Model 

A  model  state  is  given  by  the  configurations  of  all  the  objects  in  the 
environment;  tasks  are  actually  defined  by  sequences  of  states  of  the  world  model  or 
transformations  of  the  states.  The  level  of  detail  in  the  sequence  needed  to  specify 
a  task  depends  on  the  capabilities  of  the  task  planner. 

The  configurations  of  objects  needed  to  specify  a  model  state  can  be  provided 
explicitly,  as  offsets  and  Euler  angles  for  rigid  bodies  and  as  joint  parameters 
for  linkages,  but  this  type  of  specification  is  cumbersome  and  error  prone.  Three 
alternative  methods  for  specifying  configurations  have  been  developed: 

•  Use  a  light-pen  to  position  CAD  models  of  the  objects  at  the  desired 
configurations. 

•  Use  the  robot  itself  to  specify  robot  configurations  and  to  locate  features  of 
the  objects  [Grossman  and  Taylor  78). 

•  Use  symbolic  spatial  relationships  among  object  features  to  constrain  the 
configurat  ions  of  objects,  as  in  Face\  AGAINST  Face2  [Popplestone,  Ambler, 
and  Bellos  78]. 

One  advantage  of  using  symbolic  spatial  relationships  is  that  the  configurations 
they  denote  arc  not  limited  to  the  accuracy  of  a  light-pen  or  a  manipulator. 
Another  advantage  of  this  method  is  that  families  of  configurations,  such  as  those 
on  a  surface  or  along  an  edge,  can  be  expressed.  Inasmuch  as  these  relationships  are 
easy  to  interpret  by  a  human,  they  arc  easy  to  specify  and  modify.  The  principal 
disadvantage  of  using  symbolic  spatial  relationships  is  that  they  do  not  specify 


configurations  directly;  they  must  be  converted  into  numbers  or  equations  before 
they  can  be  used. 

Recall  that  model  states  are  simply  sets  of  configurations.  If  task  specifications 
were  simply  sequences  of  models,  then  given  a  method  such  as  symbolic  spatial 
relationships  for  specifying  configurations,  we  should  be  able  to  specify  tasks. 
This  approach  has  several  important  limitations.  One  limitation  is  that  a  set 
of  configurations  may  overspecify  a  state,  for  example,  a  round  pin  in  a  round 
hole.  This  problem  can  be  solved  by  treating  the  symbolic  spatial  relationships 
themselves  as  specifying  the  state,  since  these  relationships  can  express  families 
of  configurations.  A  more  fundamental  limitation  is  that  geometric  and  kinematic 
models  of  an  operation’s  final  state  are  not  always  a  complete  specification  of  the 
desired  operation.  One  example  of  this  is  the  need  to  specify  how  hard  to  tighten 
a  bolt  during  an  assembly.  In  general,  a  complete  description  of  a  task  may  need 
to  include  parameters  of  the  operations  used  to  reach  one  task  state  from  another. 

The  alternative  to  task  specification  by  a  sequence  of  model  states  is  specification 
by  a  sequence  of  operations.  Thus,  instead  of  building  a  model  of  an  object  in  its 
desired  configuration,  we  can  describe  the  operation  by  which  it  can  be  achieved. 
The  description  should  still  be  object-oriented,  not  robot-oriented;  for  example, 
the  target  torque  for  tightening  a  bolt  should  be  specified  relative  to  the  bolt  and 
not  relative  to  the  manipulator.  Most  operations  also  include  a  goal  statement 
involving  spatial  relationships  between  objects.  The  spatial  relationships  given  in 
the  goal  specification  not  only  describe  configurations;  they  indicate  the  physical 
relationships  between  objects  that  should  be  achieved  by  the  operation.  When  we  say 
that  two  surfaces  should  be  AGAINST  each  other,  for  example,  the  robot  should 
perform  a  compliant  motion  that  moves  until  the  contact  between  the  surfaces  is 
actually  detected.  This  is  quite  different  from  computing  the  position  where  the 
contact  should  have  occurred  —  assuming  perfect  knowledge  -  and  commanding 
the  robot  to  move  to  that  position.  For  these  reasons,  existing  proposals  for 
task-level  programming  languages  have  adopted  an  operation-centered  approach  to 
task  specification  [Lieberman  and  Wesley  77,  Lozano-Perez  76].  TWAIN  also  assumes 
th  3  .m  of  input. 

1.5.  Previous  work 

A  number  of  task-level  language  systems  have  been  proposed,  but  no  complete 
system  has  been  implemented.  In  this  section  we  briefly  review  these  systems. 

The  Stanford  Hand-Eye  system  [Feldman,  et  al.  71]  was  the  first  of  the 
task- level  system  proposals.  A  subset  of  this  proposal  was  implemented  [Paul  72]  in 
a  program  called  Move-Instance  that  chose  stable  grasping  positions  on  polyhedra 
and  planned  a  motion  to  approach  and  move  the  object.  The  planning  did  not 
involve  obstacle  avoidance  (except  for  the  table  surface)  or  the  planning  of  sensory 
operations. 

AL  [Finkel,  et  al.  74],  as  initially  defined,  called  for  the  ability  to  specify  models 
in  AL  and  to  allow  specification  of  operations  in  terms  of  these  models.  This  has 
been  the  subject  of  some  research  [Binford  79,  Taylor  76],  but  the  results  have  not 
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Figure  2.  Similar  prg-iii-hole  tasks  that  require  different  strategies. 

been  incorporated  into  the  existing  AL  system.  Some  additional  work  within  the 
context  of  Stanford’s  Acronym  system  [Brooks  81]  has  dealt  with  planning  grasp 
positions  [Binford  79],  but  AL  has  been  viewed  as  the  target  language  rather  than 
the  user  language. 

'faylor  l76]  discusses  an  approach  to  the  synthesis  of  sensor-based  AL  programs 
from  task-level  specifications.  Taylor’s  method  relies  on  representing  prototypical 
motion  strategies  for  particular  tasks  as  parameterized  robot  programs,  known  as 
procedure  skeieions.  A  skeleton  has  all  the  motions,  error  tests,  and  computations 
needed  to  carry  out  a  task,  but  many  of  the  parameters  needed  to  specify  motions 
and  tests  remain  to  be  specified.  The  applicability  of  a  particular  skeleton  to  a 
task  depends  on  the  presence  of  certain  features  in  the  model  and  the  values 
of  parameters  such  as  clearances  and  uncertainties.  Choices  among  alternative 
strategies  for  a  single  operation  are  made  by  first  computing  the  values  of  a  set  of 
parameters  specific  to  the  task,  such  as  the  magnitude  of  uncertainty  region  for  the 
peg  in  peg-in-hole  insertion,  and  then  using  these  parameters  to  choose  the  best,  that 
is  fastest,  strateg}’.  Having  chosen  a  strategy,  the  planner  computes  the  additional 
parameters  needed  to  specify  the  strategy  motions,  such  as  grasp  positions  and 
approach  positions.  A  program  is  produced  by  inserting  these  parameters  into  the 
procedure  skeleton  that  implements  the  chosen  strategy. 

Taylor’s  work  on  making  planning  decisions  by  manipulating  constraints  on 
positions  that  explicitiy  model  error  was  significantly  extended  by  Brooks  [82].  The 
principal  extension  was  the  use  of  these  symbolic  constraints  not  only  forward  to 
get  error  bounds  but  backward  to  restrict  the  choices  on  plan  variables  and  to 
introduce  appropriate  sensing  into  the  program.  This  approach  underlies  much  of 
TWA  ^  and  is  described  in  detail  in  Section  2. 

The  approach  to  strategy-  synthesis  based  on  procedure  skeletons  assumes  that 
task  geometry  for  common  sub-tasks  is  predictable  and  can  be  divided  into  a 
manageable  number  of  classes  each  requiring  a  different  skeleton.  This  assumption 
is  needed  because  the  sequence  of  motions  in  the  skeleton  will  be  consistent 
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of  the  two-dimensional  pcg-in-hole  problem  in  Figure  6(a)  when  the  axes  of  the  peg 
and  hole  are  constrained  to  be  parallel.  The  basic  step  in  the  synthesis  approach  is 
to  identify  ranges  of  configurations  from  where  p  can  reach  C  by  a  single  motion. 
The  directions  of  such  motions  can  be  represented  as  unit  vectors,  v,.  For  each  v,-, 
we  can  compute  all  those  configurations,  /’,  such  that  a  motion  along  v,-  from  that 
configuration  would  reach  some  point  of  G.  We  call  this  range  of  configurations 
that  can  reach  the  goal  by  a  single  motion  along  a  specified  heading  vector  the 
pre-image  of  the  goal  (for  that  vector).  .411  we  need  do  to  guarantee  that  p  reaches 
G  from  any  point  in  any  of  the  P,  is  to  use  v,-  as  the  commanded  velocity  vector 
for  a  damper. 

The  computation  of  pre-images  must  take  into  account  the  possibility  of  the 
moving  object  sticking  on  a  surface.  In  particular,  assuming  the  motion  is  generated 
by  a  damper  (with  B  =  61),  if  the  angle  between  the  commanded  velocity  and  the 
normal  of  a  surface  is  less  than  the  friction  angle  (the  arctangent  of  the  coefficient 
of  frict  ion)  then  the  motion  will  stick  on  that  surface. 

If  no  pre-irnage  of  G  contains  the  peg’s  current  configuration,  then  we  can  apply 
the  same  jire-irnage  computation  recursively  using  each  of  the  existing  pre-images 
as  a  possible  goal.  Isach  pre-image  of  G,  Pi,  serves  to  define  a  new  goal  set.  This 
process  is  repeated  until  some  pre- image  contains  some  subset  of  the  legal  start 
configurations.  From  the  chain  of  pre-images  we  can  construct  a  motion  sequence. 

One  key  problem  in  the  synthesis  method  is  to  discover  the  sequence  of 
command  velocity  vectors  Vp  Our  approach  is  to  narrow  in  on  feasible  values  of 
by  progressive  refinement.  We  start  with  the  complete  range  of  possible  v,’s  and 
remove  from  that  range  any  values  that  can  possibly  lead  to  failure  (by  sticking 
on  a  non-goal  surface  or  by  sliding  away  from  the  goal).  At  each  step  of  the 
algorithm,  we  compute  the  pre-image  of  the  goal  for  the  current  range  of  v,’s.  The 
pre-image  for  a  range  of  commanded  velocities  is  the  intersection  of  the  pre-images 
for  each  of  the  velocities.  These  are  the  configurations  guaranteed  to  reach  the 
goal  for  all  the  velocities  in  the  range.  If  the  pre-image  includes  feasible  starting 
configurations,  then  we  have  found  a  valid  motion  sequence,  otherwise  the  current 
range  of  velocities  must  be  narrowed  further. 

Figure  7  illustrates  the  method  on  a  simple  two-dimensional  block- in- corner 
example.  The  directed  graph  shown  there  has  nodes  for  each  of  the  C-surfaces  in 
the  task  and  one  node  representing  free  C-space  (C).  There  is  a  link  from  nodes 
m  to  n  in  the  graph  if  some  velocity  in  the  current  range  may  cause  the  robot  to 
move  from  some  point  in  rn  (which  is  not  in  n)  to  some  point  in  n  (which  may  be 
at  the  intersection  of  rn  and  n)  without  going  through  points  in  any  other  node. 
We  call  this  the  reachability  graph  for  that  range  of  commanded  velocities. 

In  the  example,  we  start  out  with  a  range  of  commanded  velocities  including 
any  velocity  that  will  move  p  from  nearby  points  onto  the  goal  G  (we  diagram 
ranges  of  commanded  velocities  as  sectors  of  a  circle).  The  reachability  graph  for 
this  range  of  velocities  is  shown  at  the  top  of  Figure  7.  In  this  figure,  we  have 
indicated  those  surfaces  where  the  moving  object  may  stick  (using  the  electrical 
ground  symbol).  The  (potentially)  sticking  surfaces  are  those  whose  friction  cones 
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Figure  6.  Pcg-in-)ioie:  (a)  original  formulation  (b)  transformed  to  point  problem. 

Notice  that  is  wholly  contained  in  set  B,  so  if  it  is  reached  the  gross  motion 
will  have  been  successful. 

Due  to  manipulator  inaccuracies,  straight-line  motions  will  not  necessarily  be 
achieved.  The  shaded  regions,  in  figure  5,  show  all  possible  paths,  demonstrating 
that  whatever  motion  actually  occurs  is  guaranteed  to  be  collision  free. 


3.  Fine-Motion  Synthesis 

The  problem  of  automatically  synthesizing  a  fine  motion  from  a  geometric 
description  has  received  little  attention  in  the  literature.  Previous  approaches 
were  cither  based  purely  on  skeletons  (Taylor  76,  Lozano-Perez  76]  or  on  learning 
strategies  (Dufay  and  Latombe  83].  In  this  section  we  outline  an  approach  to 
automatic  fine-motion  synthesis  initially  described  in  (Lozano-Perez,  Mason,  and 
Taylor  84]  and  further  developed  in  (Mason  84,  Erdmann  84].  This  approach  is 
based  on  the  notion  of  a  configuration  space  (Lozano-Perez  81,  83,  Brooks  and 
Lozano-Perez  83);  see  the  Appendix  for  a  brief  introduction. 

The  fine-motion  planner  constructs  a  sequence  of  motions  that  guarantees  that 
some  configuration  from  a  specified  range  of  goal  configurations  will  be  reached  from 
r-’v^vhere  within  a  computed  range  of  start  configurations.  Each  element  in  the 
mo  on  sequence  is  a  guarded  compliant  motion,  in  particular,  a  commanded  velocity 
vec.oi  for  a  generalized  damper  (Whitney  76],  and  a  termination  predicate.  The 
desired  motion  for  a  generalized  damper  is  determined  by  the  following  relationship 

f  =  B(v  —  vo) 

where  f  is  the  vector  of  forces  acting  on  the  moving  object,  vq  is  the  commanded 
velocity  vector,  and  v  is  the  actual  velocity  vector.  The  effect  of  these  compliant 
motions  is  to  slide  on  the  C-surfaces  (see  the  Appendix)  derived  from  the  obstacles. 
When  not  in  contact  with  a  surface,  the  motion  will  be  along  the  commanded 
velocity  (within  some  velocity  uncertainty).  The  motion  terminates  when  the 
associated  predicate  (a  function  of  observed  configuration,  velocity,  and  elapsed 
time)  evaluates  to  true. 

Consider  the  simple  task  of  moving  the  point  p  from  its  initial  configuration 
to  any  one  of  the  configurations  in  G  (see  Figure  6(b)).  This  is  the  C-space  version 
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Figure  5.  Gross- motion  planning. 


c.  Trajectory  constraints 

d.  Error  bounds; 

Output: 

a.  Path  (may  be  null) 

b.  Restricted  start  locations 

c.  Restricted  goal  locations. 

The  gross-motion  planner  determines  collision- free  motions  for  the  manipulator 
as  it  moves  to  the  general  location  of  an  object  in  order  to  grasp  it  and  as  it 
transfers  an  object  within  the  workspace.  The  two  classes  of  motions  can  be  treated 
by  a  single  algorithm  —  the  volume  occupied  by  the  hand  changes  depending  on 
what  is  grasped. 

The  planner  is  given  initial  and  goal  sets  of  configurations  for  the  hand.  It  is 
given  bounds  (perhaps  parameterized)  on  the  location  error  of  any  payload  relative 
to  the  hand,  and  on  the  position  control  error  of  the  manipulator.  Lastly  it  may 
be  given  constraints  which  must  be  met  by  the  trajectory.  These  are  of  two  types. 
They  can  limit  the  class  of  trajectories  considered  (e.g.  it  might  be  demanded 
that  the  payload  be  reoriented  only  about  a  vertical  axis),  or  they  can  provide  a 
criterion  for  choosing  the  trajectory  from  the  considered  class  (e.g.  minimization 
of  trajectory  length,  or  of  payload  reorientation). 

The  planner  produces  a  set  of  gross-manipulator  motions  (perhaps  parameterized 
in  terms  of  plan  variables)  which  guarantee  motion  of  the  hand  or  payload  from 
any  point  in  the  initial  set  of  configurations  to  some  point  in  the  final  set. 

Figure  5  shows  an  example  of  gross-motion  control  for  moving  a  point  in  two 
dimensions  (this  might  be  the  configuration  space  of  some  moving  object  with  a 
more  interesting  shape).  The  task  is  to  move  the  point  from  any  position  in  set  A  to 
somewhere  in  set  B.  Three  commands  are  generated:  “move  to  di”,  “move  to  d2" 
and  “move  to  d/'  Each  is  a  command  to  lower  level  servo  routines  to  move  in  a 
straight  line  to  the  desired  point  (using  pure  position  control).  Due  to  manipulator 
inaccuracies  the  true  destinations  of  the  motion  commands  will  lie  in  the  error  balls 
£>1,  Dt  and  D,{  respectively. 


error  we  can  only  specify  the  position  of  the  point  to  be  within  some  error  ball  and 
the  specified  heading  within  some  cone  around  v.  The  planner  must  pick  the  initial 
position  and  the  direction  of  motion  with  these  errors  in  mind. 

In  general,  achieving  a  task  will  require  several  motions.  For  each  of  these 
a  region  of  initial  locations  (and  corresponding  motions)  will  be  computed.  The 
planner  returns  each  of  these  regions  as  well  as  the  corresponding  motions.  These 
regions  can  serve  as  alternative  goals  for  the  gross  motion  planner. 

The  fine-motion  planner  assumes  it  has  a  complete  description  of  the 
environment.  In  TWAIN,  however,  fine-motion  planning  is  done  before  a  final 
layout  of  the  environment  is  available.  The  planner  computes  a  guard  volume  where 
the  introduction  of  another  object  would  affect  the  planned  strategy.  These  guard 
volumes  are  used  in  the  final  layout  phase. 

2.  Grasping 
Input: 

a.  Pick  up  location  of  part 

b.  Put  down  destination  of  part 

c.  Grasp  constraints 

d.  Error  bounds; 

Output: 

a.  Grasp  configurations 

b.  Grasp  motions 

c.  Guard  volume. 

The  grasp  planner  is  given  as  inputs  the  pick  up  and  put  down  locations  for 
the  part  to  be  grasped  (possibly  in  terms  of  plan  variables).  It  is  also  given  bounds 
on  the  position  error  of  the  part  at  the  pick  up  location.  As  in  fine  motion  planning, 
the  planner  may  be  given  constraints  on  where  the  part  should,  or  should  not  be 
grasped. 

he  planner  determines  where  the  part  should  be  grasped  and  the  desired 
oriei.t.iLion  of  the  hand  relative  to  the  surface.  The  planner  also  must  determine  a 
sequence  of  motions  that  guarantees  that  the  desired  grasp  configuration  is  reached 
in  the  presence  of  errors  in  manipulator  positioning  and  part  location. 

Figure  8  shows  a  simple  example  of  grasp  planning  for  a  parallel  jaw  gripper. 
Note  that  the  choice  of  where  to  place  the  fingers  depends  not  only  on  the  part, 
but  also  on  the  environment  near  the  part  at  the  initial  and  final  locations.  As 
with  fine  motion  planning,  the  grasp  planner  specifies  a  guard  volume  in  which  the 
presence  of  objects  would  require  pre-planning. 

3.  Gross  Motion 
Input: 

a.  Start  locations 

b.  Goal  locations 
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1.  Gross  Motion 

2.  Grasp 

3.  Bolt  in  hole 

4.  Ungrasp 

5.  Dead  reckon  vertical  align  (i.e.  stacking) 

6.  Catch-all  synthesized  fine  motion 

2.5.  The  planner  modules  in  more  detail 

The  three  planning  modules  are  briefly  described  here  in  general  form.  In 
sections  3  through  5  of  the  proposal  we  refer  to  published  work  which  describes 
implementations  of  specializations  of  each  of  the  module  descriptions. 

All  modules  have  access  to  a  current  context  world  model,  which  has 
specifications  via  geometric  descriptions  and  algebraic  constraints  of  all  the  parts 
whose  locations  have  been  decided  exactly  or  at  least  been  constrained  to  some  set 
of  possibilities. 

1.  Fine  Motion 
Input: 

a.  Initial  locations  of  parts 

b.  Goal  specification 

c.  Assembly  constraints 

d.  Error  bounds; 

Output: 

a.  Compliant  motion  strategy 

b.  Legal  initial  locations  for  each  motion 

c.  Guard  volume. 

The  fine-motion  planner  is  given  as  inputs  the  sets  of  possible  initial  locations 
for  the  parts  and  a  specification  of  the  range  of  legal  final  configurations.  It  is  also 
given  bounds  on  the  errors  in  measuring  the  manipulator’s  position  and  the  forces 
acting  on  it,  and  bounds  on  the  error  in  controlling  the  position  and  velocity  of  the 
manipulator.  Lastly,  it  may  be  given  constraints  on  the  motions,  such  constraints 
might  forbid  hitting  some  surfaces  or  exceeding  bounds  on  forces. 

The  planner  determines  compliant  motions  which  ensure  that  the  parts  will 
reach  one  of  the  final  configurations  if  started  from  within  the  specified  range  of 
initial  locations.  Typically  the  planning  process  will  also  place  additional  constraints 
on  the  legal  initial  locations. 

Figure  7  shows  an  example  of  fine-motion  planning  for  a  point  in  two  dimensions. 
This  is  actually  the  configuration  space  of  a  simple  block-in-corner  task  (see  the 
Appendix).  The  task  is  to  move  the  point  p  from  its  initial  location  to  somewhere 
in  set  G.  A  single  compliant  motion  along  the  direction  v  from  anywhere  within 
the  shaded  subset  of  A  will  reach  some  part  of  G.  Note  that  due  to  manipulator 


which  is  trivially  true, 

-0.20  <  B-POSITION  -  A-POSITION  <  0.2 

and 

nomina/(B-POSITION)  =  nomina/(A- POSITION), 
while  the  propagation  constraints  are: 

-0.05  <  B-POSITION  -  A- RESULT-POSITION  <  0.05 

and 

nomma;(B-POSITION)  =  nomtnal(A-RESULT-POSITION). 

The  physical  quantities  are  expanded  into  plan  and  uncertainty  components  using 

A-POSITION  =  B-NOM  -f  A-UNC 
B-POSITION  =  B-NOM  -f  B-UNC 
A-RESULT-POSITION  =  B-NOM  -h  A-RESULT-UNC 
where  the  equalities  in  the  constraints  are  used  to  substitute  equal  values.  The 
choice  of  using  B-NOM  as  the  nominal  value  for  all  three  physical  quantities  is  for 
readability  only  —  internally  it  is  more  likely  to  be  something  like  G0487.  The 
constraint  equalities  have  dictated  that  the  peg  should  be  nominally  aligned  with 
the  hole  before  the  insertion.  This  is  manifested  by  the  peg  and  hole  being  given 
the  same  nominal  value  in  this  plan  step.  The  constraint  propagator  forces  this  to 
be  true  by,  for  instance,  specifying  the  nominal  end  point  of  the  previous  gross 
motion  that  moves  peg  to  the  vicinity  of  the  hole. 

Now  the  applicability  constraints  reduce  to; 

-0.20  <  B-UNC -A-UNC  <  0.2 
and  the  propagation  constraints  to: 

-0.05  <  B-UNC -A-RESULT-UNC  <  0.05. 

The  methods  developed  in  (Brooks  82]  can  be  used  to  find  ways  of  guaranteeing 
that  the  applicability  constraint  are  met.  For  instance  it  may  be  necessary  to  sense 
tlij  .sition  of  the  hole  before  a  previous  plan  step  of  moving  the  peg  to  a  nominal 
posi  ion  above  the  hole.  The  sense  step  would  give  a  new  more  accurate  estimate 
of  the  location  of  the  hole  giving  a  new  B-NOM  value  and  a  better  bound  on  the 
possible  values  for  B-UNC. 

Given  a  range  of  values  for  B-UNC  the  propagation  constraint  determines  a 
range  of  values  for  A-RESULT-UNC,  the  uncertainty  in  peg  position  aRer  insertion. 
Later  steps  in  the  plan  may  need  to  examine  this  range  to  see  whether  it  meets 
their  applicability  constraints. 

Skeletons  for  many  other  actions  can  be  produced  similarly.  They  need  not 
be  used  only  to  model  actions  in  detail.  The  constraint  sets  might  be  very 
underconstraining  allowing  minimal  requirements  for  a  large  class  of  actions  to  be 
simultaneously  considered.  Such  skeletons  must  later  be  replaced  by  detailed  plans 
produced  by  one  of  the  specialist  planning  modules. 

A  plausible  small  library  of  skeletons  includes: 
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Figure  4.  BolUin-liole  example. 

Step  modeled  by  the  skeleton  is  guaranteed  to  succeed.  The  propagation  constraints 
put  bounds  on  the  possible  physical  situations  that  can  be  produced  by  the  plan 
step  given  any  initial  situation  satisfying  the  applicability  constraints. 

Consider  a  skeleton  to  describe  insertion  of  a  peg  into  a  hole.  For  simplicity 
we  will  assume  a  two-dimensional  world,  ignoring  the  details  of  vertical  motions 
even  in  that  world.  To  match  a  physical  situation  and  plan  step  there  must  be  a 
bolt  and  a  hole.  The  applicability  constraint  set  consists  of 

PEG-SHAFT-RADIUS  <  HOLE-RADIUS, 

PEG-TIP-RADIUS  -  HOLE- RADIUS 
<HOLE-POSITION  -  PEG-POSITION 
<HOL^ RADIUS  -  PEG- TIP-RADIUS, 

and 

nominal(HOLE-POSITION)  ^  nomino/(PEG-POSITION). 

The  propagation  constraint  set  is  simply: 

PEG- SHAFT- RADIUS  -  HOLE^ RADIUS 
<HOLE-POSITION  -  PEG-RESULT-POSITION 
<HOLE- RADIUS  -  PEG-FHAFT-RADIUS 
and 

nomzna/(HOLE^POSITION)  =  nommoI(PEG-RESULT-POSITION). 

The  meta-function  nominal  refers  to  the  planned  value  for  a  physical  quantity. 

Now  consider  the  physical  situation  shown  in  figure  4.  The  peg  is  labeled  A 
and  the  hole  is  labeled  B.  In  this  example,  both  the  peg  and  hole  have  definite 
radii.  After  matching  the  skeleton  to  the  physical  situation,  the  constraint  sets  are 
instantiated  as;  the  applicability  constraints: 

0.2  <  0.25, 
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now  back  through  fine-rnotion  steps,  and  perhaps  introduce  sensing  steps  to  achieve 
the  pre-conditions  necessary  for  the  success  of  such  steps.  Failure  during  constraint 
propagation  can  only  be  due  to  a  synthesized  fine  motion  having  excessively  strong 
pre-conditions.  In  that  case  planning  is  backed  up  to  phase  6  for  a  new  fine  motion 
synthesis. 

10.  Choose  r  ctual  physical  locations  for  the  initial  location  of  every  object  and 
for  the  nominal  locations  to  be  used  in  all  intermediate  steps  of  the  plan.  This  is  the 
detailed  layout  of  the  workspace.  On  failure  due  to  clutter  of  some  work  area  by 
numerous  objects  and  tasks  (such  as  may  happen  in  an  area  where  the  manipulator 
is  most  accurate)  backup  to  phase  5  for  a  new  gross  layout.  The  result  will  be  that 
new,  more  difficult,  fine  motion  syntheses  will  have  to  be  carried  out  which  will 
succeed  in  areas  with  less  accurate  manipulator  characteristics.  On  failure  due  to 
the  impossibility  of  keeping  a  guard  volume  free  for  the  hand  return  to  phase  8  for 
the  next  available  grasp. 

11.  Plan  collision-free  motions  for  the  manipulator  to  achieve  each  gross  change 
in  location  of  objects,  and  to  move  the  manipulator  from  the  release  of  one  object 
to  the  acquisition  of  the  next.  If  gross-motion  planning  fails  then  backup  to  one  of 
phases  5  (gross  layout),  8  (grasp  analysis)  or  10  (detailed  layout),  depending  on  the 
reason  for  failure.  If  no  path  could  be  found  for  the  payload  due  to  re-orientation 
difficulties  then  variously  try  tweaking  detailed  layout  in  phase  10  (such  as  making 
wider  corridors  if  free  space  at  the  failed  path  segments)  or  retry  the  grasp  analysis 
at  phase  8  (such  as  selecting  candidate  grasps  which  place  a  widely  different  part  of 
the  object  along  center  of  rotation  of  the  last  manipulator  joint  axis).  If  collisions 
with  the  upperarm  or  forearm  were  the  main  problem  then  backup  to  phase  5  for 
a  new  gross  layout  of  the  workspace. 

2.4.  Skeletons 

A  plan  skeleton  models  a  step  in  a  plan  in  terms  of  its  inputs  and  outputs. 
The  idea  is  that  the  details  of  the  plan  step  can  be  treated  as  a  black  box  by  the 
rest  of  the  plan  —  the  rest  being  affected  only  by  the  input/output  behavior  of  an 
in  li  aual  plan  step. 

Sl'.eletons  can  describe  plan  steps  at  different  levels  of  detail.  For  instance 
there  might  be  a  skeleton  for  pickup  used  early  in  the  planning  process.  Later  in 
the  process  the  action  might  be  modeled  by  a  series  of  four  finer  plan  steps  each 
instantiated  by  a  skeleton,  gross-manipulator  motion,  fine-manipulator  motion, 
grasp,  and  another  gross-manipulator  motion. 

A  skeleton  is  specified  by  a  geometric  description  of  objects  and  their  state  in 
a  world  and  by  two  sets  of  constraints:  a  set  of  applicability  constraints  and  a  set  of 
propagation  constraints.  The  skeleton  is  instantiated  by  finding  a  match  between 
the  geometric  description  and  the  known  world  state.  Both  sets  of  constraints  in 
the  skeleton  are  expressed  in  variables  which  get  instantiated  by  physical  quantity 
variables  as  a  by-product  of  the  geometric  matching. 

Once  instantiated,  the  feasibility  of  using  such  a  plan  step  can  be  considered. 
If  the  applicability  constraints  are  satisfied  by  a  physical  situation,  then  the  plan 
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level  of  abstraction.  It  is  essentially  a  branch- and- bound  search  and  later  it  may 
turn  out  that  some  bounds  were  incorrect  and  more  sensing  will  be  necessary  in 
non-SFM  steps  than  is  produced  at  this  point.  If  a  fine-motion  skeleton  (non-SFM) 
causes  failure,  then  the  skeleton  generator  for  that  motion  is  re-invoked,  a  new 
skeleton  is  selected,  and  this  phase  is  repeated. 

5.  We  now  have  some  constraints  on  many  of  the  locations.  With  these  we  do 
a  coarse  layout,  identifying  large  reasonable  areas  to  place  planned  locations.  For 
each  SFM  step  the  manipulator  error  is  analyzed  over  these  large  areas,  which  may 
be  partitioned  into  subsets  having  widely  different  error  ball  upper  bounds.  The 
partitions  are  ordered  smallest  error  bound  first,  and  a  list  of  pairs  of  error  balls 
and  work  areas  is  formed.  These  will  be  used  to  drive  generators  for  syntheses  of 
fine-motion  strategies.  If  all  possible  gross  layouts  have  already  been  tried  then  the 
planner  fails. 

6.  For  each  SFM  generator  look  at  its  first  error  ball  and  work  area,  but  do 
not  remove  them  from  the  generator  list.  If  an  actual  fine-motion  synthesis  (see 
section  3)  has  not  yet  been  done  for  this  plan  step  then  do  it.  The  result  is  a 
series  of  sets  R  from  which  the  synthesized  fine  motion  is  guaranteed  to  succeed 
(see  section  3).  The  fine- motion  planner  also  generates  a  guard  volume  where  the 
motion  will  happen.  This  volume  is  the  subject  of  the  layout  phase  (phase  10).  If 
an  SFM  generator  is  already  exhausted  then  backup  to  phase  5  for  a  new  gross 
layout. 

7.  Using  branch-and-bound  search  on  cost  estimates  propagate  constraints 
through  the  plan  with  the  old  SFM  skeletons  replaced  by  actual  fine  motions. 
Choose  the  smallest  set  R  (of  initial  locations)  for  each  fine  motion  that  can  be 
guaranteed  to  be  reached  (i.e.  the  manipulator  error  ball  fits  inside  it).  If  some  fine 
motion  causes  plan  failure  then  remove  it  from  its  generator  and  backup  to  phase 
6  for  a  new  synthesis. 

8.  For  each  object  that  must  be  grasped  generate  a  list  of  possible  grasp 
configurations,  ordered  by  criteria  such  as  firmness  of  grip  and  accessibility  of  the 
grasp  surfaces.  A  grasp  depends  on  both  the  initial  configuration  of  the  object  and 
its  goal  configuration.  The  interaction  of  the  hand  with  the  fine  motion  at  the  end 
of  any  gross  motion  of  the  object  must  also  be  considered.  On  failure  to  find  any 
grasp  at  all  some  new  terminal  strategy  must  be  necessary,  so  backup  to  phase  6 
for  a  new  fine-motion  synthesis.  The  actual  positions  of  the  grasp  might  depend  on 
plan  variables.  Each  grasp  produces  a  guard  volume  which  must  be  free  of  obstacles 
so  that  the  hand  can  fit  in  a  position  necessary  to  achieve  the  grasp.  Note  that 
the  guard  volume,  appropriately  translated  and  perhaps  re-oriented,  must  be  free 
both  at  pick  up  and  at  put  down.  The  first  grasp  in  the  list  of  grasps  is  used  in 
subsequent  planning  phases,  although  they  may  back  up  to  this  phase  for  a  new 
grasp. 

9.  Propagate  constraints  through  the  plan  moving  forward  until  some  pre¬ 
condition  constraints  are  not  met,  and  then  backing  up  to  introduce  sensing  or 
otherwise  constrain  plan  variables.  This  phase  is  essentially  a  repeat  of  phase  4, 
except  that  we  have  more  detailed  instantiations  of  each  plan  step.  Thus  we  can 


•  Fail  giving  a  reason  and  perhaps  incorporating  a  suggestion  to  change  the 

given  situation. 

•  Succeed,  producing  a  detailed  plan  and  perhaps  a  further  set  of  constraints 

which  should  be  applied  to  the  given  situation  in  order  to  refine  it  further. 

Furthermore,  each  planning  module  can  act  as  a  generator  of  success/restriction 
pairs.  As  it  is  re-invoked  it  generates  another  solution  or,  eventually,  fails. 
The  generators  define  the  search  space  for  the  constraint-propagation- triggered 
backtracking.  The  restrictions  associated  with  each  solution  give  the  backtracking 
dependency  direction  [Stallman  and  Sussman  77]. 

2.3.  Control  structure 

The  task-level  plan  specification  consists  of  a  series  of  changes  in  location  for 
objects  in  the  workspace.  These  can  be  deduced  by  comparing  consecutive  world 
descriptions.  The  task  planner  then  proceeds  with  the  following  phases: 

1.  The  executive  turns  each  motion  into  a  sequence  consisting  of 

a.  Gross  manipulator  motion 

b.  Grasp 

c.  Gross  manipulator  motion 

d.  Fine  motion 

e.  Ungrasp. 

2.  Every  one  of  these  plan  steps  is  then  instantiated  by  a  skeleton.  This  is 
done  by  the  skeleton  matcher.  It  is  guaranteed  to  find  a  skeleton  which  matches. 
All  gross  manipulator  motions  are  matched  by  the  same  skeleton  as  are  grasps  and 
ungrasps.  Some  fine  motions  will  be  matched  by  specific  skeletons  (such  as  "bolt  in 
hole”)  and  those  that  are  not,  are  matched  by  a  catch-all  skeleton  for  synthesized 
fine  motion  (SFM).  Skeleton  matching  includes  identification  of  different  physical 
locations.  Each  location  is  associated  with  the  plan  step  where  it  first  gains  physical 
m^a  '..ig  (e.g.,  the  position  of  an  object  on  a  table  first  becomes  real  during  the 
put-  down  fine  motion  of  the  manipulator). 

3.  A  dependency  analysis  is  carried  out  for  locations,  that  is,  it  is  determined 
which  physical  placements  of  objects  depend  on  others.  The  analysis  forms  multiple 
fibers  running  through  a  linear  arrangement  of  plan  steps  —  sometimes  skipping 
over  one  or  more  steps. 

4.  Pre-condition  and  propagation  constraints  are  propagated  through  the  plan 
from  one  plan  step  to  another.  Sometimes  the  pre-conditions  (called  applicability 
constraints)  will  not  be  guaranteed  to  be  satisfied.  In  that  case  backing  up  may  be 
necessary.  The  dependency  graph  is  followed.  Backing  up  can  introduce  sensing. 
Skeletons  for  synthesized  fine  motion  cannot  be  backed  through  directly.  It  is 
possible,  however,  to  make  certain  inferences  concerning  whether  sensing  will  be 
necessary  and  minimum  bounds  on  costs  of  sensing.  These  are  used  to  direct  the 
backtracking.  Note  that  this  phase  does  not  guarantee  a  workable  plan,  even  at  this 


The  facts  about  the  manipulator  mentioned  above  when  applied  to  the  physical 
action  of  moving  the  box  to  destination  give  two  constraints  on  the  plan  and 
uncertainty  variable.  The  reach  of  the  manipulator  determines  plausible  planned 
locations  for  the  box,  i.e. 

5  <  BOX-NOM  <  20 

and  the  manipulator  accuracy  determines  bounds  on  the  uncertainty  in  the  physical 
location  of  the  box  as  a  function  of  the  commanded  position  for  it,  i.e. 

-0.2  +  0.005  X  BOX-NOM  <  BOX-UNC  <  0.2  -  0.005  X  BOX-NOM.  (4) 

Given  a  suitable  inference  engine,  such  as  described  in  [Brooks  81],  we  can  now 
make  deductions  useful  for  the  planning  process.  For  instance,  no  matter  which 
legal  vilue  is  chosen  for  the  nominal  box  position,  the  uncertainty  about  where  it 
will  actually  be  placed  is  no  bigger  than  ±0.175.  Conversely,  if  during  the  planning 
proces.'^  it  becomes  necessary  to  ensure  that  the  position  of  the  box  is  known  to 
±0.15  then  two  possibilities  are  that  the  nominal  position  be  constrained  by 

10  <  BOX-NOM 

or  by  introducing  a  sense  operation  immediately  after  placement  of  the  box  to  see 
where  it  was  placed.  In  the  latter  case  the  sensor  must  be  chosen,  and  the  nominal 
value  of  the  box  position  may  need  to  be  constrained,  so  that  the  box  will  be  within 
a  region  where  the  sensor  error  is  bounded  by  ±0.15. 

Geometric  models,  sets  of  equalities  such  as  (3)  and  inequalities  such  as  (4) 
combine  to  form  representations  of  classes  of  physical  situations.  Each  physical 
situation  in  a  class  corresponds  to  one  or  more  points  in  the  satisfying  set  of  all 
the  inequalities.  Such  points  with  different  values  for  plan  variables  correspond  to 
different  refinements  of  a  plan.  Points  with  identical  values  for  all  plan  variables,  but 
different  values  for  uncertainty  variables  correspond  to  different  physical  realizations 
of  a  single  planned  situation. 

2.2.  The  modules 

The  task  planner  consists  of  a  constraint  propagator  and  a  skeleton  matcher 
along  with  three  planning  modules  (other  modules  might  be  added).  The  planning 
modules  are  a  fine-molion  planner,  a  grasp  planner,  and  a  gross-motion  planner. 
The  planner  also  has  a  library  of  plan  skeletons. 

Constraints  are  propagated  between  plan  steps,  providing  the  propagator  with 
constraints  on  layout  and  enabling  it  to  introduce  sensing  steps  when  necessary. 
Initially  plan  steps  are  modeled  by  instantiated  plan  skeletons  and  later  refined 
into  more  detail  by  the  planning  modules.  Constraint  propagation  continues  as  the 
plan  is  refined,  propagating  the  inter-relating  constraints  back  and  forth  between 
plan  steps. 

Bi  oadly  speaking,  each  planning  module  is  given  a  class  of  situations  for  which 
it  must  produce  a  detailed  plan  step.  The  planning  module  can; 

•  Fail  with  no  reason  given. 


BOX  POSITION 


Figure  3.  Definition  of  BOX- POSITION 

Uncertainty  variables  represent  quantities  whose  values  can  never  be  known, 
even  at  plan  execution  time;  usually  they  represent  the  dificrence  between  a  physical 
quantity  and  the  nominal  value  which  will  be  chosen  for  it  by  the.  planner.  Thus 
they  represent  uncertainties  in  the  planner’s,  and  plan  executor’s,  knowledge  of 
the  state  of  the  world.  Although  their  values  can  never  be  known,  bounds  on  their 
magnitudes  can  either  be  known  a  priori  (e.g.  the  manufacturing  tolerance  of  a 
workpiece)  or  can  be  deduced  by  reasoning  about  the  motions  and  sense  operations 
leading  up  to  the  establishment  of  the  appropriate  physical  quantity. 

To  illustrate  these  concepts  consider  the  two-dimensional  situation  shown 
in  figure  3.  A  workpiece  is  to  be  placed  somewhere  on  a  table  by  a  two-link 
manipulator.  Suppose  the  error,  c  in  the  i  direction  made  by  the  manipulator  in 
trying  to  place  the  box  with  a  planned  x  coordinate  p*  is  given  by 

-0.2  -f  0.005  X  pj  <  e  <  0.2  -  0.005  X  P*.  (1) 

Further,  suppose  that  the  working  range  of  the  manipulator  is  such  that  commanded 
positions  for  the  box  must  satisfy 

5  <  Pi  <  20.  (2) 

We  represent  the  constraints  so  placed  on  the  physical  situation  by  using  three 
x-Kbles: 

BOX-POSITION:  a  physical  quantity  variable  representing  the  actual  physical 
value  which  will  be  achieved  as  the  x  coordinate  of  where  the  box  will  be  placed. 

BOX-NOM:  a  plan  variable  representing  the  nominal  value  which  will  be 
chosen  during  planning  and  will  be  passed  to  the  robot  controller  as  the  destination 
position  commanded  for  the  manipulator’s  motion,  while  grasping  the  box. 

BOX-UNC:  an  uncertainty  variable  representing  the  error  which  will  be  made 
by  the  manipulator  in  placement  of  the  box. 

The  above  definitions  imply 

BOX-POSITION  =  BOX-NOM  -f  BOX-UNC.  (3) 

This  provides  a  link  from  the  geometric  representation  of  the  world,  where 
BOX-POSITION  is  a  parameter,  to  the  constraint  language  which  will  be  used  in 
reasoning  during  the  planning  process. 


a  complete  set  of  constraints  without  making  some  decisions  within  individuil  plan 
steps.  Hence  the  plan  will  fail  if  these  decisions  are  incorrect.  The  planner  therefore 
needs  to  employ  a  backtracking  mechanism  where  decisions  can  be  made  and  later 
undone  in  case  of  failure. 

The  TWAIN  approac*'  is  first  to  model  all  plan  steps  at  a  broad  level  (as 
instantiated  plan  skeletons)  and  generate  all  constraints  which  the  steps  imply  for 
every  one  of  their  possible  refinements.  The  effects  of  the  constraints  are  propagated 
throughout  the  complete  plan.  Each  plan  step  is  then  refined  into  further  detail. 
Again  constraints  are  propagated  and,  in  case  of  failure,  dependency-directed 
backtracking  occurs. 

The  order  in  which  plan  steps  are  refined  depends  on  the  type  of  operation 
they  describe  and  their  sensitivity  to  decisions  made  elsewhere  in  the  plan.  For 
exam  ole,  gross  motion  planning  is  done  last  as  it  is  almost  completely  insensitive 
to  any  fine- motion  strategies,  or  sensing  operations  planned  elsewhere.  On  rare 
occasions  the  choice  of  sensor  location  or  jig  layout  for  some  force-directed  motion 
strategy  might  block  the  workspace  and  make  gross  motion  planning  impossible. 
On  the  other  hand  the  position  of  a  workpiece  relative  to  a  sensor  can  affect 
whether  the  sensor  can  make  useful  measurements  on  it.  Thus  sensing  operations 
should  be  planned  before  the  workspace  is  layed  out,  so  that  constraints  from  those 
operations  can  be  taken  into  account. 

2.1.  Constraints  as  a  communication  mechanism 

To  achieve  our  goals,  it  is  ncccessary  to  represent  and  manipulate  geometric 
constraints  and  to  distinguish  between  that  which  is  not  yet  decided  in  the 
planning  process  and  that  which  cannot  be  known  even  at  plan  execution  time 
due  to  manipulator  error,  sensor  error,  and  parts  tolerances.  TWAIN  uses  a  scheme 
presented  by  [Brooks  82). 

Constraints  are  represented  by  inequalities  on  explicit  expressions  over  formal 
variables.  These  variables  are  of  three  types:  physical  quantity  variables,  plan 
variables,  and  uncertainty  variables.  There  are  two  legal  classes  of  expressions  over 
these  types:  expressions  which  can  include  both  plan  and  uncertainty  variables, 
and  expressions  containing  only  physical  quantities.  The  two  classes  of  expressions 
can  appear  on  opposite  sides  of  an  inequality  or  an  equality.  The  semantics  of  the 
variables  are  as  follows. 

Physical  quantity  variables  represent  actual  physical  quantities  in  the  real 
world.  They  are  used  to  label  quantities  in  the  geometric  world  model. 

Plan  variables  represent  quantities  whose  values  must  be  decided  by  the  time 
of  plan  execution.  Values  for  them  are  chosen  as  part  of  the  planning  process. 
Plan  variables  provide  a  mechanism  for  deferring  decisions  during  planning,  and 
constraints  including  plan  variables  provide  a  representation  for  reasoning  about 
the  implications  of  how  those  decisions  will  turn  out.  Often  plan  variables  are  used 
to  represent  nominal  values  for  physical  quantities. 


only  with  a  particular  class  of  geometries.  The  assumption  does  not  seem  to  be 
true  in  general.  In  particular,  the  presence  of  additional  surfaces  in  tasks  may 
generate  unexpected  contacts,  leading  to  failures.  This  approach  is  in  contrast  to 
an  approach  which  derives  the  strategy  directly  from  consideration  of  the  task 
description  [Lozano-Perez,  Mason,  and  Taylor  83].  In  the  TWAIN  design,  both  types 
of  approaches  play  a  role. 

The  LAMA  system  was  designed  at  MIT  [Lozano-Perez  76,  Lozano-Perez  and 
Winston  77]  as  a  task-level  language,  but  only  partially  implemented.  LAMA 
formulated  the  relationship  of  task  specification,  obstacle  avoidance,  grasping, 
skeleton-based  strategy  synthesis,  and  error  detection  within  one  system.  More 
recent  work  at  MIT  has  explored  issues  in  task  planning  in  more  detail  outside  of 
the  context  of  any  particular  system  [Brooks  82,  83a,  83b,  Brooks  and  Lozano-Perez 
83,  Lozano-Perez  81,  83,  Lozano-Perez,  Mason,  and  Taylor  84,  Mason  81,  82]. 

AUTOPASS,  at  IBM  [Lieberman  and  Wesley  77],  defined  the  syntax  and  semantics 
of  a  task-level  language  and  an  approach  to  its  implementation.  A  subset  of  the 
most  general  operation,  the  PLACE  statement,  was  implemented.  The  major  part  of 
the  implementation  effort  focused  on  a  method  for  planning  collision-free  paths  for 
Cartesian  robots  among  polyhedral  obstacles  [Lozano-Perez  and  Wesley  79]. 

RAPT  [Popplestone,  Ambler,  and  Bellos  78]  is  an  implemented  system  for 
transforming  symbolic  specifications  of  geometric  goals,  together  with  a  program 
which  specifies  the  directions  of  the  motions  but  not  their  length,  into  a  sequence  of 
end-effector  positions.  RAPT’s  emphasis  has  been  primarily  on  task  specification;  it 
has  not  dealt  yet  with  obstacle-avoidance,  automatic  grasping,  or  sensory  operations. 

Some  robot-level  language  systems  have  proposed  extensions  to  allow  some  task- 
level  specifications.  LM-GEO  is  an  implemented  extension  to  LM  [Latombe  and  Mazer 
81]  which  incorporates  symbolic  specifications  of  destinations.  The  specification 
of  ROBEX  [Week  and  Zuhlke  81]  includes  the  ability  to  plan  automatically  collision- 
free  motions  and  to  generate  programs  that  use  sensory  information  available 
during  execution.  A  full-blown  ROBEX,  including  these  capabilities,  has  not  been 
implemented. 

2.  Overview  of  the  twain  System 

The  geometry  of  the  world  determines  how  TWAIN  refines  a  given  sequence  of 
object  motions  into  a  detailed  plan,  including  sensing  steps,  of  action  to  be  carried 
out  by  a  manipulator. 

Because  of  the  complex  interactions  between  plan  steps,  TWAIN  must  refine 
each  step  with  as  little  commitment  as  possible  to  decisions  within  that  refinement 
until  the  effects  of  decisions  made  in  refining  other  plan  steps  are  known.  One 
approach  might  be  to  generate  all  the  constraints  that  each  plan  step  implies  for 
other  steps,  and  ultimately  pick  a  set  of  motions  and  sense  operations  satisfying 
all  those  constraints.  However,  because  of  the  number  of  possibilities  in  refining  a 
single  step  and  the  inter-dependencies  between  steps,  wc  cannot  in  general  obtain 
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Figure  7.  Block-in-corncr  fine-motion  synthesis. 

overlap  the  current  velocity  range.  No  pre- image  exists  for  the  initial  velocity  range 
because  of  the  possibility  of  sticking  on  either  A  or  B.  By  removing  velocities  that 
may  cause  sticking  on  surface  A  or  B  from  the  current  range  of  velocities,  we  obtain 
a  velocity  range  for  which  a  pre-image  {€')  exists.  This  pre-image  intersects  the 
initial  range  of  configurations,  so  a  successful  motion  has  been  found. 

The  example  above  can  be  done  with  a  single  motion.  We  did  not  require 
recursive  calls  to  the  planner.  In  general,  we  have  a  choice  of  refining  the  range 
of  directions  or  of  using  the  current  pre- image  as  the  goal  for  a  recursive  call  to 
tht  s  me  algorithm.  This  choice  at  each  step  defines  the  search  space  of  motion 
sequ'  nces.  Another  important  aspect  of  the  approach  is  the  synthesis  of  termination 
predicates  for  the  motions.  These  issues  are  further  discussed  in  [Lozano-Perez, 
Mason,  and  Taylor  84,  Mason  84,  Erdmann  84]. 

4.  Grasping 

The  problem  of  choosing  a  grasp  point  on  an  object  has  received  significant 
attention  in  the  literature  (Paul  72,  Lozano-Perez  76,  81  W’ingham  77,  Brou  80, 
Laugier  81,  Mason  82,  Laugier  and  Pertin  83].  The  approach  to  grasping  described 
here  is  based  on  that  described  in  [Lozano-Perez  81];' a  more  detailed  treatment  can 
be  found  there.  This  approaches  also  based  on  the  notion  of  configuration  space; 
see  Appendbe. 
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The  grasp  planner  chooses  which  object  surfaces  will  be  grasped  and  builds  a 
description  of  the  grasp  configurations  (on  those  surfaces)  that  satisfy  the  following 
constraints; 

1.  The  inside  of  the  fingers  are  in  contact  with  surfaces  of  P,  the  object  to 
be  grasped. 

2.  There  are  no  collisions  between  the  manipulator  hand  and  any  nearby 
objects  for  any  possible  start  configuration  of  P. 

3.  There  are  no  collisions  between  the  manipulator  hand  and  any  nearby 
objects  for  any  possible  goal  configuration  of  P. 

4.  The  grasp  is  stable,  i.e.,  can  withstand  forces  generated  during  motion 
and  assembly. 

We  assume  that  the  manipulator  hand  is  a  parallel  jaw.  We  further  assume 
that  the  manipulator  can  be  partitioned  into  an  arm  and  hand.  The  arm  serves 
to  place  the  wrist  at  any  point  in  the  workspace;  the  hand  determines  the  final 
configuration  of  the  gripper.  This  is  a  common  kinematics  for  manipulators  and  has 
a  number  of  theoretical  and  practical  advantages.  The  grasp  planner  determines 
candidate  hand  configurations;  the  gross-motion  planner  must  then  pick  some  hand 
configuration  that  allows  finding  a  collision-free  path  from  the  start  to  the  goal. 

The  choice  of  grasp  surfaces  is  done  by  ranking  the  surfaces  by  their  likelihood 
of  providing  a  stable  grasp  and  then  choosing  the  highest  ranked  surface  that  leads 
to  a  feasible  grasp  configuration.  A  general  treatment  of  stability  in  grasping  is 
not  yet  ava ’.able,  although  some  promising  approaches  exist  [Hanafusa  and  Asada 
77].  When  the  object  to  be  grasped  is  small  relative  to  the  manipulator  hand,  two 
simple  heuristics  provide  a  fair  chance  of  identifying  a  stable  grasp  (see  also  [Paul 
72,  Brou  80]).  The  heuristics  are: 

1.  Ensure  at  least  a  minimum  contact  area  of  the  fingers  with  the  grasp 
surfaces.  The  amount  of  overlap  should  depend  on  object  properties  such 
as  weight  and  surface  smoothness. 

2.  The  perpendicular  projection  of  P’s  center  of  mass  should  be  near  to  the 
contact  area  of  the  fingers  and  grasp  surfaces. 

The  grasp  planner  computes  feasible  grasp  configurations  for  the  top  ranked 
candidate  grasp  surfaces.  Note  that  because  the  manipulator  configuration  will 
change  while  moving  P  from  its  start  to  its  goal  configuration,  we  represent  the 
grasp  configurations  as  the  configuration  of  the  hand  relative  to  P.  We  can  impose 
restrictions  that  reduce  the  dimensionality  of  the  set  of  grasp  configurations.  One 
simple  restriction,  for  parallel  jaw  hands,  is  to  require  that  at  least  one  of  the 
surfaces  grasped  be  planar  (the  other  may  be  a  planar  surface,  curved  surface,  edge, 
or  vertex). 

Let  Pi  be  the  planar  face  of  P  to  be  grasped,  Py  is  the  other  face  (edge  or 
vertex),  and  Pi  and  F2  be  the  inside  faces  of  the  manipulator’s  fingers.  Under  the 
restriction  stated  above,  when  P  is  grasped,  either  Pi  or  P2  is  coplanar  with  P,- 
(without  loss  of  generality  assume  P|  is  coplanar  with  P,).  Under  these  conditions, 


the  legal  {x,y,z]  positions  of  all  points  on  the  hand  are  restricted  to  be  on  some 
plane  parallel  to  P,  .  The  hand  may  rotate  about  the  normal  to  this  plane.  Let  G  be 
the  set  of  configurations  of  the  hand  for  which  P,-  and  Pj  are  coplanar.  G  is  called 
the  grasp  set  for  P,-. 

Not  all  the  configurations  in  G  are  feasible  grasp  configurations,  either  because 
tni  angers  are  not  in  contact  with  the  grasp  surfaces  or  because  the  corresponding 
m  nipulator  configuration  causes  a  collision  (at  the  start  or  at  the  goal).  The 
constraint  that  the  fingers  touch  the  grasp  surfaces  can  be  readily  enforced  by 
restricting  the  grasp  set  to  be  the  intersection  of  those  hand  configurations  for 
which  Pj  overlaps  P,  and  those  for  which  P2  overlaps  Py.  This  intersection  set  can 
be  computed  explicitly  in  low- dimensional  C-space  jLozano-Perez  81,  83,  Brooks 
and  Lozano- Perez  83).  Similarly,  those  hand  configurations  (defined  relative  to  P) 
that  cause  collisions  with  objects  at  the  start  or  at  the  goal  can  be  computed.  The 
grasp  set  G  can  then  be  intersected  with  their  complement  to  obtain  the  set  of 
feasible  grasp  configurations. 

Figure  8  shows  an  example  of  the  feasible  grasp  computation:  (a)  the  pick-up 
and  put-down  orientations  of  the  hand,  (b)  C-space  obstacles  at  pick  up  (the  shaded 
region  is  the  accesiblc  part  of  the  grasp  set),  (c)  C-space  obstacles  at  put  down,  (d) 
put  down  obstacles  constraining  the  grasp  set  further. 
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If  P  is  free  to  move  during  the  grasping  operation  and  its  initial  position  is 
not  known  to  high  accuracy,  then  the  grasp  planner  must  take  into  consideration 
the  possible  motions  during  planning.  This  is  quite  difficult  in  general;  see  [Mason 
82]. 

Many  grippers  currently  under  development,  such  as  Salisbury’s  three-fingered 
hand  and  the  Utah-MIT  hand,  are  much  more  complex  than  the  parallel  jaw  gripper 
we  have  been  considering.  The  planning  method  above,  although  still  relevant, 
becomes  vastly  more  complex  for  these  multi-fingered  hands.  This  is  an  area  where 
more  research  is  desperately  needed. 


5.  Gross  Motion 

After  a  trickle  of  early  work  on  collision-free  gross-motion  planning,  there  has 
been  a  avalanche  of  new  ideas  and  developments  recently. 

The  earliest  reasonably  general  algorithms  for  manipulators  were  for  the 
Stanford  arm  (it  has  one  sliding  and  five  revolute  joints).  One  was  implemented 
[Udupa  77]  and  the  other  partially  implemented  [Widdoes  74].  Both  relied  on 
approximations  for  the  payload,  limited  wrist  action,  and  tcsselation  of  joint  space 
to  describe  forbidden  and  free  regions  of  real  space.  The  problem  with  tesselation 
schemes  is  that  to  get  adequate  motion  control  a  multi-dimcnsional  space  must  be 
finely  tesselated. 

Lozano-Perez  [81]  presented  an  implemented  algorithm  for  Cartesian  manipu¬ 
lators.  Cartesian  manipulators  have  three  sliding  joints  whose  axes  are  orthogonal 
and  thus  they  can  be  used  as  the  axes  of  space  representation.  Lozano-Perez’s 
algorithm  used  configuration  space  for  the  Cartesian  portion  of  the  manipulator 
(where  the  natural  Euclidean  axes  of  the  configuration  space  correspond  exactly  to 
the  joints  of  the  manipulator)  and  subdivided  ranges  of  angles  for  the  hand,  within 
each  of  which  a  bounding  volume  for  the  hand  and  payload  is  used. 

Schwartz  and  Sharir  [82]  have  shown  that  the  problem  is  polynomial  in  the 
number  of  obstacle  surfaces  for  a  manipulator  with  a  fixed  number  of  joints.  If  n  is 
the  number  of  obstacle  surfaces,  the  running  time  of  their  algorithm  is  ©(n®”*)  for  a 
six  degree-of-freedom  manipulator.  It  is,  therefore,  primarily  of  theoretical  interest 
and  not  meant  to  be  implemented. 

Brooks  and  Lozano-Perez  [83]  have  developed  a  practical  and  implemented 
algorithm  for  polygonal  obstacle  avoidance  which  produces  paths  which  require 
arbitrarily  difficult  rotations  up  to  a  preset  resolution. 

Brooks  [83a]  developed  a  new  representation  for  two-dimensional  free  space 
as  overlapping  freeways.  This  has  led  [Brooks  83b]  to  the  development  and 
implementation  of  a  gross  motion  algorithm  for  picic  and  place  operations  for  a 
manipulator  with  revolute  joints. 

The  key  idea  is  that  free  space  should  be  explicitly  represented  in  such  terms 
that  it  is  easy  to  determine  the  collision- free  motion  segments  that  can  be  made 
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by  the  manipulator  and  its  payload.  Individual  legal  motion  segments  are  linked  to 
form  a  complete  motion  for  the  manipulator. 


See  figure  9  for  a  diagram  of  the  PUMA.  Brooks’  algorithm  decomposes  the 
problem  by  spending  two  degrees  of  freedom  of  the  manipulator  to  partially  decouple 
the  payload  and  the  upperarm  of  the  manipulator.  In  the  six  degree-of-freedom 
PUMA  it  keeps  joint  4  fixed  (there  is  no  joint  4  for  the  5  degree-of-freedom  PUMA) 
and  uses  joint  5  to  compensate  for  the  payload  orientation  for  the  motions  of  the 
upper  and  forearms.  Joint  6  is  free  to  re-orient  the  payload  about  its  vertical  axis, 
but  such  re-orientation  does  not  require  motion  of  either  the  upper  or  the  forearm 
—  it  is  completely  decoupled.  This  is  only  two-dimensional  rotation.  There  is  still 
coupling  of  translations  of  the  payload  and  the  motion  of  the  upper  links  of  the 
arm.  A  major  new  contribution  of  the  algorithm  is  that  motion  of  the  components 
can  at  first  be  analyzed  separately  and  then,  later,  constraints  are  propagated 
bcuw  en  the  solutions  to  account  for  the  remaining  coupling. 

Ine  algorithm  finds  paths  where  the  payload  is  moved  in  straight  lines,  either 
horizontal  or  vertical,  and  is  only  re-oriented  by  rotations  about  the  vertical  axis 
of  the  world  coordinate  system.  Thus  only  4  degrees  of  freedom  are  considered  for 
the  PUMA. 

The  payload  and  the  hand  are  merged  geometrically,  and  the  payload  is 
considered  to  be  a  prism,  with  convex  cross  section.  The  payload  can  rotate  about 
the  vertical,  as  joint  6  rotates. 

Obstacles  in  the  work  space  are  of  two  types;  those  supported  from  below  and 
those  hanging  from  above.  Both  are  prisms  with  convex  cross  sections.  Non-convex 
obstacles  can  be  modeled  by  overlapping  prisms.  Prisms  can  be  supported  from 
below  if  they  rest  on  the  workspace  table  or  on  one  another  as  long  as  they  are 
fully  supported.  Thus  no  point  in  free  space  ever  has  a  bottom  supported  obstacle 
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above  it.  Sucli  ob.st.icle  dcseriplion.s  have  been  extracted  I’roin  depth  measurements 
from  a  stereo  pair  of  overhead  cameras.  The  algorithm  has  generated  collision  free 
paths  from  that  data. 

Similar  pro  defuied  obstacles  may  also  hang  from  above  intruding  into  the 
workspace  of  the  iipperarm  and  forearm.  Obstacles  are  precluded  from  a  cylinder 
surrounding  the  manipulator  base. 

The  class  of  motions  allowed  suffice  for  many  assembly  operations,  and,  with 
as  yet  unknown  algorithms  for  re  orienting  the  payload  without  major  arm  motion, 
the  algorithm  could  provide  gross  motion  planning  for  all  but  the  most  diHicult 
realistic  problems. 

6.  Conclusions 

In  this  proposal  we  have  outlined  an  architecture  for  a  new  task-level  system, 
which  wo  call  1  W.MN.  Our  goal  has  been  to  dcGne  a  unified  framework  for  existing 
and  future  research  on  task  planning.  We  have  summarized  approaches  to  several  of 
the  key  problems  in  task  planning:  fine  motion  synthesis,  grasping,  and  gross  motion 
planning.  These  areas  arc  relatively  mature.  Some  other  areas  such  as  automatic 
parts  layout,  feeding,  and  lixturing  have  received  significantly  less  attention.  We 
prcpo.se  to  ccnstract  a  prototype  implementation  of  TWAIN  during  the  next  two 
years  v/here  the  focus  will  be  on  the  interaction  between  the  modules  described 
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Figure  10.  Geometric  conditions  giving  rise  to  C-surfaces. 


Appendix:  Configuration  Space 


A  configuration  of  an  object  is  the  set  of  parameters  needed  to  specify 
completely  the  position  of  all  points  of  the  object.  The  configuration  of  a  rigid 
two-dimension^'  object,  for  example,  can  be  specified  by  two  displacements  and  an 
angle,  that  of  a  rigid  three-dimensional  object  by  three  displacements  and  three 
angles,  and  that  of  a  robot  arm  by  its  joint  angles.  For  concreteness,  we  will  be 
dealing  exclusively  with  Cartesian  configurations,  e.g.,  [x,y,6)  for  objects  in  the 
plane,  and  not  joint  angle  configurations.  The  space  of  all  possible  configurations  for 
an  object  is  known  as  the  configuration  space  (C-space)  of  that  object  [Lozano-Perez 
81,  83].  An  object  A  is  represented  as  a  point  in  its  C-space;  the  coordinates  of 
that  point  are  the  configuration  parameters  of  A. 

Stationary  obstacles  in  the  environment  of  a  moving  object  A  can  be  mapped 
into  the  configuration  space  of  A.  The  resulting  C-space  obstacles  are  those 
configurations  of  A  which  would  lead  to  collisions  between  A  and  the  obstacles. 
Configurations  on  the  surface  of  the  C-space  obstacle  due  to  B  are  those  where  some 
surface  of  A  is  just  touching  a  surface  of  B.  If  A  and  B  are  both  three-dimensional 
polyhedra,  the  surfaces  of  the  C-space  obstacle  for  B  arise  from  each  of  the 
feasible  contacts  between  the  vertices,  edges,  and  faces  of  A  and  B  (see  figure  10) 
iLozano-Perez  83).  Therefore,  each  face  of  a  C-space  obstacle  represents  a  particular 
t>’pe  of  geometric  constraint  on  A.  A  range  of  positions  (and  orientations)  of  A  can 
be  represented  as  a  volume  in  the  C-space  of  A  and  a  motion  of  A  is  a  curve  in  the 
C-space. 

As  an  illustration  of  the  use  of  C-space  surfaces,  consider  the  familiar  two- 
dimensional  peg- in-hole  problem  from  figure  6.  'We  can  construct  a  three- dimensional 
C-space  of  {x,y,6)  configurations  of  the  peg.  In  this  space,  the  hole  defines  an 
obstacle  (see  figure  11(a)).  Note  that  although  the  resulting  surfaces  are  curved, 
for  each  value  of  6  the  (i,  y)  cross  section  of  the  C-space  surfaces  is  polygonal. 
The  surfaces  represent  one-point  contacts  and  the  edges  at  the  intersections  of 
surfaces  represent  two-point  contacts.  Line-line  contacts  also  give  rise  to  edges  at 
the  intersections  of  one-point  contact  surfaces.  Figure  11(b)  shows  cross  sections 
for  a  peg  and  chamfered  hole. 
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Figure  11.  Cross  sections  of  peg-in-hole  C-surfaces:  (a)  no  chamfer  (b)  chamfer. 

The  C-space  representation  can  be  extended  to  more  general  kinematic 
situations.  In  general,  motion  subject  to  geometric  and  kinematic  constraints  can 
be  defined  as  collections  of  equalities  and  inequalities  that  must  hold  among  the 
parameters  that  determine  the  configurations  of  the  robot  and  the  objects  in  the 
task.  These  inequalities  represent  C-surfaces  [Mason  81].  Take  the  constraint  that 
a  robot  hand  remain  in  contact  with  a  crank  handle  as  it  rotates.  The  constraint 
relating  the  position  of  the  hand,  (z,  y),  to  the  position  of  the  crank  (a  constant) 
and  its  current  angle,  a,  is  a  curve  (one-dimensional  surface)  in  the  configuration 
•pace  of  the  t2isk,  i.e.,  the  (z,y,a)  space. 
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